#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,           sensorI2CCustom9V)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motorH,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
#include "ballhoop definitions.h"
#include "drivers/HTPB-driver.h"
#include "basic.h"

//////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                              NXT Brick User Controlled Button Example.
//
// By default the NXT firmware has standard processing for the four buttons on the NXT. This
// program illustrates how the standard processing can be over-ridden and user defined actions
// can be processed within user's Robotc program.
//
//                                     *  ********  *
//                                   * *  *      *  * *
//                                 * L *  * Enter*  * R *
//                                   * *  *      *  * *
//                                     *  ********  *
//
//                                        ********
//                                        * Exit *
//                                        ********
//
// The four buttons are shown in the above graphic: Top line is left, enter, right buttons
//                                                  second line is 'exit' button.
//
// There are two approaches to perform button management in a user program.
//  1. The user program can continuously poll the button states and look for changes.
//  2. The user program can be notified whenever a button is pushed. The notification is by
//     starting a user specified task running.
//  For both methods, you have to set some variables to override the default system button
//  handler.
//
//  There are three intrinsic variables defined for NXT button management.
//
//  nNxtButtonPressed   This read-only variable contains the currently pressed button. [The NXT
//                      firmware only supports recognition of one button at a time]. The value
//                      is one of the following:
//                           typedef enum
//                           {
//                             kNoButton      = 0xFF,
//                             kExitButton    = 0,
//                             kRightButton   = 1,
//                             kLeftButton    = 2,
//                             kEnterButton   = 3
//                           } TButtonMasks;
//
//  nNxtButtonTask  This variable contains the task number that should be started whenever any NXT
//                   button is pushed. There are three value ranges:
//                       -1       Indicates that the normal system processing for exit button should
//                                be used. Anything else will over-ride the system actions. Note if
//                                overwritten, you will not be able to stop program running with button
//                                control.
//                      1..9      Indicates that the indicated task (1..9) should be started whenever
//                                the exit button is pressed. Task will only be started once per
//                                keypress.
//                      Other			Standard processing is overwritten. No task will be started when exit
//                                button is pushed; user program must used nNxtButtons to check for
//
//  nNxtExitClicks  Contains the number of 'clicks' that must be received to abort a running program.
//                  The default value is one. But if you want to use the EXIT button in your program
//                  you can set this to 2 (for a double click) or 3 (triple click) or higher so that
//                  there is always an escape sequence, independent of your user program, for existing
//                  your program.
//
//                  This variable is reset to 1 before the start of any program.
//
//                  If you really want to override the 'abort' check, you can continually modify this
//                  variable. The count of current EXIT clicks is reset whenever this variable is
//                  written.
//
//
// It may be easier to use the method of starting a task whenever a button is pressed. The user program
// can look at the 'nNxtButtonPressed' variable to determine which button has been pressed.
//
// How to Terminate/Abort A Program when Buttons are Hijacked
// ==========================================================
// When user program has gained control of the NXT buttons, the regular processing of the buttons is stopped.
// This means the EXIT button will not be recognized as a command to stop the end user program. So there
// must be an escape sequence to allow for this. Several different schemes were tried and the following
// was finally adopted for RobotC.
//
// A program is aborted by counting the number of "clicks" on the EXIT button. The normal default case is
// that a single click will abort a program. Setting this to a higher value (double or tripe clicks) will
// still allow user program to use EXIT button, but firmware will still have the capability to abort
// program
//
//
// Notes on NXT Hardware:
// ======================
//  1. Buttons on the NXT are not individually read. Instead there is are several resistors in series that
//     are shorted by pressing the individual keys. This is fed into a single input pin where an A-to-D
//     conversion is performed. Analyzing the result indicates which key is pressed.
//  2. A byproduct of this is that the NXT can only properly handle a single keypress at a time. If
//     multiple keys are pressed, then the firmware recognizes the key that generates the lowest resistance.
//     Only this key is reported to the user program.
//  3. A byproduct is that you cannot design a user interface that tries to use two keys depressed at the
//     same time.
//  4. The firmware recognizes button depresses in the following order(low to high): left, right, enter, exit.
//
///////////////////////////////////////////////////////////////////////////////////////////////////////

//
// TesT craTe lifT
//  lefT buTTon = CRAtE 1 POSItION
//  enTer buTTon =HOME POSItION
//  righT buTTon =CRAtE 2 POSItION
//  exiT buTTon = SPIN POSItION
//
int nLeftButton  = 0;
int nRightButton = 0;
int nEnterButton = 0;
int nExitButton  = 0;
int curpos,tarpos;


task main()
{
servo[servo1] = CLAMP_OPEN;
StartTask (cratelift);
// while(true)
// {
 // EndTimeSlice();
// }
int nCycles = 0;
	//
	// "Hi-jack" buttons for user program control.
	//
//	nNxtButtonTask  = ButtonTask;

	//
	// Note: program cannot be terminated if we hijack the 'exit' button. So there has to be an escape sequence
	//       that will return buttons to system control! We'll use a triple click
	//
	nNxtExitClicks  = 2; // Double clicking EXIT button will terminate program
	//
	// Do nothing. Just keep waiting
	//
	while (true)
	{
		//
		// Some dummy code to do some work. In this case we'll simply display a count of the number of times
		// each button has been pressed.
		//
	  string sTemp;
   // curpos = HTPBreadADC(HTPB, 0, 10); // read position from potentiometer at A0

    //nxtDisplayTextLine(1, "curpos = %d",curpos);

	  switch (nNxtButtonPressed)
    {
    	case kLeftButton:
    	{
    	  if(cltarget != CRATE1)
    	  {
    	    nxtDisplayTextLine(0, "CRATE 1 Position");
          nxtDisplayTextLine(2, "tarpos = %d",CRATE1);
        }
        cltarget = CRATE1;
  	    break;
  	  }
		  case kRightButton:
    	  if(cltarget != CRATE2)
    	  {
		      nxtDisplayTextLine(0, "CRATE 2 Position");
          nxtDisplayTextLine(2, "tarpos = %d",CRATE2);
        }
        cltarget = CRATE2;
  	    break;

	  	case kEnterButton:
	  	  if(cltarget != CLHOME)
    	  {
	  	    nxtDisplayTextLine(0, "HOME Position");
          nxtDisplayTextLine(2, "tarpos = %d",CLHOME);
        }
        cltarget = CLHOME;
  	    break;


		  case kExitButton:
	  	  if(cltarget != CLHOME)
    	  {
		      nxtDisplayTextLine(0, "SPIN Position");
	  	    nxtDisplayTextLine(2, "tarpos = %d",SPIN);
        }
	  	  cltarget = SPIN;
  	    break;

	  	default:
	      nxtDisplayTextLine(3, "delpos = %d",cltarget-curpos);
	    switch(cltarget)
	    {
		    case CRATE1:
		    case CRATE2:
			    if (cltargetreached)
			      servo[servo1] = CLAMP_HOME;  //clamp
			    break;

			  case CLHOME:
        if(abs(delta) <= ESP)
			      servo[servo1] = CLAMP_OPEN;
			      cltargetreached = false;
			    break;
			  }

        eraseDisplay();
		  break;
	  }
	  EndTimeSlice();
  }
	return;
}
